#!/usr/bin/env python
# -*-coding:utf-8 -*-

__all__ = ['ObstaclesPool', '__init__', 'add', 'obstacleExistInTheWay']

class ObstaclePool(object):
    def __init__(self):
        """
        创建一个障碍物池。
        """
        self._obstacles = []
        
    def add(self, obstacle):
        """
        往障碍物池中添加一个障碍物。
        :param obstacle: 待添加的障碍物（Obstacle对象）。
        """
        self._obstacles.append(obstacle)
        
    def obstacleExistInTheWay(self, position, velocity, detection_range, detection_angle):
        """
        判断在（机器人的）行进道路上是否存在障碍物。
        :param position: （机器人的）位置（Vector对象）。
        :param velocity: （机器人的）速度（Vector对象）。
        :param detection_angle: （机器人所能）检测到障碍物的角度。
        :param detection_range: （机器人所能）检测到障碍物的距离。
        """
        i = 0
        while i < len(self._obstacles):
            if self._obstacles[i].isDisappeared():
                del self._obstacles[i]
            elif self._obstacles[i].isExisting():
                vec = self._obstacles[i].getPosition()-position
                if vec.norm() < detection_range and \
                   vec.angleWith(velocity) < 0.5 * detection_angle:
                    return True
                i += 1
            else: # the obstacle is not existed yet
                i += 1
        return False